
#include "tracker.h"
#include "trackingdevice.h"


namespace Tracking
{
	Tracker::Tracker()
	{

		_worldToEmitter = mat4f::IDENTITY;

	}
	
	Tracker::~Tracker()
	{
		for(int i=0;i<trackingDevices.size();i++)
		{
			trackingDevices[i]->uninit();
			delete trackingDevices[i];
			trackingDevices.erase(trackingDevices.begin()+i);

		}
	}

	bool Tracker::addTrackingDevice(TrackingDevice* device)
	{
		for(int i=0;i<trackingDevices.size();i++)
		{
			if(trackingDevices[i]->getDeviceType() == device->getDeviceType())
				return false;
		}
		trackingDevices.push_back(device);
		
		return true;
	}
	
	bool Tracker::init()
	{
		for(int i=0;i<trackingDevices.size();i++)
		{
			if(!trackingDevices[i]->init())
				return false;
		}
		return true;
	}

	bool Tracker::isRunning()
	{

		for(int i=0;i<trackingDevices.size();i++)
		{
			if(trackingDevices[i]->isRunning())
				return true;
		}
		return false;
	
	}

	bool Tracker::update()
	{	
		bool ret = true;
		for(int i=0;i<trackingDevices.size();i++)
		{			
			if(!trackingDevices[i]->update())
				ret =false;
		}
		return ret;
	}

	TrackingDevice* Tracker::getTrackingDevice(DeviceType type)
	{
		for(int i=0;i<trackingDevices.size();i++){
			if(trackingDevices[i]->getDeviceType() == type)
				return trackingDevices[i];
		}
	}

	mat4f Tracker::transformMatrix(const mat4f& matrix, DeviceType type)
	{
		if(matrix==vmml::mat4f::IDENTITY)
			return vmml::mat4f::IDENTITY;
		else
		return _worldToEmitter * matrix * getTrackingDevice(type)->getSensorToObject();
	}



}